{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "Fggh7br8yCuj"
      },
      "source": [
        "## Installations and Importing"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "id": "YxY_uK-rxVwg"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Requirement already satisfied: roboticstoolbox-python in /usr/local/lib/python3.10/dist-packages (1.1.1)\n",
            "Requirement already satisfied: numpy>=1.17.4 in /home/luqman/.local/lib/python3.10/site-packages (from roboticstoolbox-python) (1.26.4)\n",
            "Requirement already satisfied: spatialmath-python>=1.1.5 in /usr/local/lib/python3.10/dist-packages (from roboticstoolbox-python) (1.1.10)\n",
            "Requirement already satisfied: spatialgeometry>=1.0.0 in /usr/local/lib/python3.10/dist-packages (from roboticstoolbox-python) (1.1.0)\n",
            "Requirement already satisfied: pgraph-python in /usr/local/lib/python3.10/dist-packages (from roboticstoolbox-python) (0.6.2)\n",
            "Requirement already satisfied: scipy in /home/luqman/.local/lib/python3.10/site-packages (from roboticstoolbox-python) (1.11.4)\n",
            "Requirement already satisfied: matplotlib in /home/luqman/.local/lib/python3.10/site-packages (from roboticstoolbox-python) (3.7.5)\n",
            "Requirement already satisfied: ansitable in /usr/local/lib/python3.10/dist-packages (from roboticstoolbox-python) (0.9.10)\n",
            "Requirement already satisfied: swift-sim>=1.0.0 in /usr/local/lib/python3.10/dist-packages (from roboticstoolbox-python) (1.1.0)\n",
            "Requirement already satisfied: rtb-data in /usr/local/lib/python3.10/dist-packages (from roboticstoolbox-python) (1.0.1)\n",
            "Requirement already satisfied: progress in /usr/local/lib/python3.10/dist-packages (from roboticstoolbox-python) (1.6)\n",
            "Requirement already satisfied: typing-extensions in /home/luqman/.local/lib/python3.10/site-packages (from roboticstoolbox-python) (4.5.0)\n",
            "Requirement already satisfied: pre-commit in /home/luqman/.local/lib/python3.10/site-packages (from spatialmath-python>=1.1.5->roboticstoolbox-python) (3.3.3)\n",
            "Requirement already satisfied: websockets in /home/luqman/.local/lib/python3.10/site-packages (from swift-sim>=1.0.0->roboticstoolbox-python) (11.0.2)\n",
            "Requirement already satisfied: colored<1.5.0 in /usr/local/lib/python3.10/dist-packages (from ansitable->roboticstoolbox-python) (1.4.4)\n",
            "Requirement already satisfied: contourpy>=1.0.1 in /home/luqman/.local/lib/python3.10/site-packages (from matplotlib->roboticstoolbox-python) (1.1.1)\n",
            "Requirement already satisfied: cycler>=0.10 in /home/luqman/.local/lib/python3.10/site-packages (from matplotlib->roboticstoolbox-python) (0.11.0)\n",
            "Requirement already satisfied: fonttools>=4.22.0 in /home/luqman/.local/lib/python3.10/site-packages (from matplotlib->roboticstoolbox-python) (4.42.1)\n",
            "Requirement already satisfied: kiwisolver>=1.0.1 in /home/luqman/.local/lib/python3.10/site-packages (from matplotlib->roboticstoolbox-python) (1.4.5)\n",
            "Requirement already satisfied: packaging>=20.0 in /home/luqman/.local/lib/python3.10/site-packages (from matplotlib->roboticstoolbox-python) (23.1)\n",
            "Requirement already satisfied: pillow>=6.2.0 in /home/luqman/.local/lib/python3.10/site-packages (from matplotlib->roboticstoolbox-python) (9.5.0)\n",
            "Requirement already satisfied: pyparsing>=2.3.1 in /home/luqman/.local/lib/python3.10/site-packages (from matplotlib->roboticstoolbox-python) (3.1.1)\n",
            "Requirement already satisfied: python-dateutil>=2.7 in /home/luqman/.local/lib/python3.10/site-packages (from matplotlib->roboticstoolbox-python) (2.8.2)\n",
            "Requirement already satisfied: six>=1.5 in /home/luqman/.local/lib/python3.10/site-packages (from python-dateutil>=2.7->matplotlib->roboticstoolbox-python) (1.16.0)\n",
            "Requirement already satisfied: cfgv>=2.0.0 in /home/luqman/.local/lib/python3.10/site-packages (from pre-commit->spatialmath-python>=1.1.5->roboticstoolbox-python) (3.3.1)\n",
            "Requirement already satisfied: identify>=1.0.0 in /home/luqman/.local/lib/python3.10/site-packages (from pre-commit->spatialmath-python>=1.1.5->roboticstoolbox-python) (2.5.24)\n",
            "Requirement already satisfied: nodeenv>=0.11.1 in /home/luqman/.local/lib/python3.10/site-packages (from pre-commit->spatialmath-python>=1.1.5->roboticstoolbox-python) (1.8.0)\n",
            "Requirement already satisfied: pyyaml>=5.1 in /home/luqman/.local/lib/python3.10/site-packages (from pre-commit->spatialmath-python>=1.1.5->roboticstoolbox-python) (6.0)\n",
            "Requirement already satisfied: virtualenv>=20.10.0 in /home/luqman/.local/lib/python3.10/site-packages (from pre-commit->spatialmath-python>=1.1.5->roboticstoolbox-python) (20.23.0)\n",
            "Requirement already satisfied: setuptools in /usr/local/lib/python3.10/dist-packages (from nodeenv>=0.11.1->pre-commit->spatialmath-python>=1.1.5->roboticstoolbox-python) (58.2.0)\n",
            "Requirement already satisfied: distlib<1,>=0.3.6 in /home/luqman/.local/lib/python3.10/site-packages (from virtualenv>=20.10.0->pre-commit->spatialmath-python>=1.1.5->roboticstoolbox-python) (0.3.6)\n",
            "Requirement already satisfied: filelock<4,>=3.11 in /home/luqman/.local/lib/python3.10/site-packages (from virtualenv>=20.10.0->pre-commit->spatialmath-python>=1.1.5->roboticstoolbox-python) (3.12.2)\n",
            "Requirement already satisfied: platformdirs<4,>=3.2 in /home/luqman/.local/lib/python3.10/site-packages (from virtualenv>=20.10.0->pre-commit->spatialmath-python>=1.1.5->roboticstoolbox-python) (3.5.3)\n"
          ]
        }
      ],
      "source": [
        "!pip3 install roboticstoolbox-python"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "_h69MndOAhEn"
      },
      "source": [
        "### Installation and importing "
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {
        "id": "V95jYG7q7R-E"
      },
      "outputs": [],
      "source": [
        "\n",
        "import math\n",
        "from spatialmath.base import *\n",
        "from spatialmath import SE3\n",
        "import spatialmath.base.symbolic as sym\n",
        "import numpy as np\n",
        "import roboticstoolbox as rtb"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "rhJRulJsTguA"
      },
      "source": [
        "## Robotic Arm Model PUMA-560"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "s5EwOTExWjTf",
        "outputId": "70706ad1-3ca3-4f5d-d0f9-f643a0d1647c"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "DHRobot: Puma 560 (by Unimation), 6 joints (RRRRRR), dynamics, geometry, standard DH parameters\n",
            "┌────┬────────┬────────┬────────┬─────────┬────────┐\n",
            "│θⱼ  │   dⱼ   │   aⱼ   │   ⍺ⱼ   │   q⁻    │   q⁺   │\n",
            "├────┼────────┼────────┼────────┼─────────┼────────┤\n",
            "│ q1\u001b[0m │ 0.6718\u001b[0m │      0\u001b[0m │  90.0°\u001b[0m │ -160.0°\u001b[0m │ 160.0°\u001b[0m │\n",
            "│ q2\u001b[0m │      0\u001b[0m │ 0.4318\u001b[0m │   0.0°\u001b[0m │ -110.0°\u001b[0m │ 110.0°\u001b[0m │\n",
            "│ q3\u001b[0m │   0.15\u001b[0m │ 0.0203\u001b[0m │ -90.0°\u001b[0m │ -135.0°\u001b[0m │ 135.0°\u001b[0m │\n",
            "│ q4\u001b[0m │ 0.4318\u001b[0m │      0\u001b[0m │  90.0°\u001b[0m │ -266.0°\u001b[0m │ 266.0°\u001b[0m │\n",
            "│ q5\u001b[0m │      0\u001b[0m │      0\u001b[0m │ -90.0°\u001b[0m │ -100.0°\u001b[0m │ 100.0°\u001b[0m │\n",
            "│ q6\u001b[0m │      0\u001b[0m │      0\u001b[0m │   0.0°\u001b[0m │ -266.0°\u001b[0m │ 266.0°\u001b[0m │\n",
            "└────┴────────┴────────┴────────┴─────────┴────────┘\n",
            "\n",
            "┌─┬──┐\n",
            "└─┴──┘\n",
            "\n",
            "┌─────┬─────┬──────┬───────┬─────┬──────┬─────┐\n",
            "│name │ q0  │ q1   │ q2    │ q3  │ q4   │ q5  │\n",
            "├─────┼─────┼──────┼───────┼─────┼──────┼─────┤\n",
            "│  qr\u001b[0m │  0°\u001b[0m │  90°\u001b[0m │ -90°\u001b[0m  │  0°\u001b[0m │  0°\u001b[0m  │  0°\u001b[0m │\n",
            "│  qz\u001b[0m │  0°\u001b[0m │  0°\u001b[0m  │  0°\u001b[0m   │  0°\u001b[0m │  0°\u001b[0m  │  0°\u001b[0m │\n",
            "│  qn\u001b[0m │  0°\u001b[0m │  45°\u001b[0m │  180°\u001b[0m │  0°\u001b[0m │  45°\u001b[0m │  0°\u001b[0m │\n",
            "│  qs\u001b[0m │  0°\u001b[0m │  0°\u001b[0m  │ -90°\u001b[0m  │  0°\u001b[0m │  0°\u001b[0m  │  0°\u001b[0m │\n",
            "└─────┴─────┴──────┴───────┴─────┴──────┴─────┘\n",
            "\n"
          ]
        }
      ],
      "source": [
        "## Printing its DH table\n",
        "p560 = rtb.models.DH.Puma560()\n",
        "print(p560)\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "gdYL79q4ZoeS"
      },
      "source": [
        "### Forward Kinematics"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 6,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "WY_NvV_VBnl-",
        "outputId": "a75a9000-73bf-4373-d7f2-358e1b6412a2"
      },
      "outputs": [
        {
          "data": {
            "text/plain": [
              "  \u001b[38;5;1m 1       \u001b[0m \u001b[38;5;1m 0       \u001b[0m \u001b[38;5;1m 0       \u001b[0m \u001b[38;5;4m 0.4521  \u001b[0m  \u001b[0m\n",
              "  \u001b[38;5;1m 0       \u001b[0m \u001b[38;5;1m 1       \u001b[0m \u001b[38;5;1m 0       \u001b[0m \u001b[38;5;4m-0.15    \u001b[0m  \u001b[0m\n",
              "  \u001b[38;5;1m 0       \u001b[0m \u001b[38;5;1m 0       \u001b[0m \u001b[38;5;1m 1       \u001b[0m \u001b[38;5;4m 1.104   \u001b[0m  \u001b[0m\n",
              "  \u001b[38;5;244m 0       \u001b[0m \u001b[38;5;244m 0       \u001b[0m \u001b[38;5;244m 0       \u001b[0m \u001b[38;5;244m 1       \u001b[0m  \u001b[0m\n"
            ]
          },
          "execution_count": 6,
          "metadata": {},
          "output_type": "execute_result"
        }
      ],
      "source": [
        "p560.fkine([0, 0, 0, 0, 0, 0])    # solving the forward kinematic on given angles."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "Ifasq-9BUkC8",
        "outputId": "39842c3e-7aeb-416e-b786-a26c2a4a8d7f"
      },
      "outputs": [
        {
          "data": {
            "text/plain": [
              "  \u001b[38;5;1m-0.1777  \u001b[0m \u001b[38;5;1m-0.9684  \u001b[0m \u001b[38;5;1m-0.175   \u001b[0m \u001b[38;5;4m-0.2547  \u001b[0m  \u001b[0m\n",
              "  \u001b[38;5;1m 0.9577  \u001b[0m \u001b[38;5;1m-0.211   \u001b[0m \u001b[38;5;1m 0.1955  \u001b[0m \u001b[38;5;4m 0.5096  \u001b[0m  \u001b[0m\n",
              "  \u001b[38;5;1m-0.2263  \u001b[0m \u001b[38;5;1m-0.1329  \u001b[0m \u001b[38;5;1m 0.965   \u001b[0m \u001b[38;5;4m 0.9699  \u001b[0m  \u001b[0m\n",
              "  \u001b[38;5;244m 0       \u001b[0m \u001b[38;5;244m 0       \u001b[0m \u001b[38;5;244m 0       \u001b[0m \u001b[38;5;244m 1       \u001b[0m  \u001b[0m\n"
            ]
          },
          "execution_count": 30,
          "metadata": {},
          "output_type": "execute_result"
        }
      ],
      "source": [
        "q=[40, 50, 0,90, 0, 10]\n",
        "p560.fkine(q)    # solving the forward kinematic on given angles."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "HT_KCzNsBcNz"
      },
      "source": [
        "### Inverse Kinematics "
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 7,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "KTV2rqraMRb0",
        "outputId": "bc65ccfa-7dc9-4a00-8952-478a949a3049"
      },
      "outputs": [
        {
          "data": {
            "text/plain": [
              "IKSolution(q=array([ 2.21921185, -2.41679954,  4.75643145, -6.2831853 , -2.33966827,\n",
              "        4.06397346]), success=False, iterations=3000, searches=100, residual=0.011737243353156889, reason='iteration and search limit reached')"
            ]
          },
          "execution_count": 7,
          "metadata": {},
          "output_type": "execute_result"
        }
      ],
      "source": [
        "point = SE3(0.6, -0.5, 0.0)\n",
        "point_sol = p560.ikine_LM(point)\n",
        "point_sol"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "shjAOhcEGU_g"
      },
      "source": [
        "### Trajectory Generation"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 8,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "HTU5Kxf2Gbtf",
        "outputId": "b3a37143-653e-47d3-a463-55717645576b"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Each row contains 6 joint angles and we have 200 of them\n"
          ]
        },
        {
          "data": {
            "text/plain": [
              "(200, 6)"
            ]
          },
          "execution_count": 8,
          "metadata": {},
          "output_type": "execute_result"
        }
      ],
      "source": [
        "p560 = rtb.models.DH.Puma560()\n",
        "t = np.arange(0, 2, 0.010) # creating 200 steps\n",
        "point_1 = SE3(0.6, -0.5, 0.0) ## starting point in 3D space\n",
        "point_2 = SE3(0.4, 0.5, 0.2)## ending point in 3D space\n",
        "Ts = rtb.tools.trajectory.ctraj(point_1, point_2, len(t)) ## Calculating trajectory between points\n",
        "sol = p560.ikine_LM(Ts) ## Generating Inverse kinematics solution for 200 points\n",
        "print(\"Each row contains 6 joint angles and we have 200 of them\" )\n",
        "sol.q.shape"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "9fZywEoFU7AH"
      },
      "source": [
        "### Making things Symbolic for **Generating Equations**"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 9,
      "metadata": {
        "id": "a63188ZmBy5I"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "(q_0, q_1, q_2, q_3, q_4, q_5)\n"
          ]
        }
      ],
      "source": [
        "p560_symblic = rtb.models.DH.Puma560(symbolic=True)\n",
        "# Creating Symbolic variables for angles -> to obtain equations\n",
        "q = sym.symbol('q_:6')\n",
        "print(q)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 10,
      "metadata": {
        "id": "Axwf167TDXKq"
      },
      "outputs": [],
      "source": [
        "symbolic_trasformation_matrix = p560_symblic.fkine(q)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "f0WAEnp9hHjS"
      },
      "source": [
        "------------------------------------------------------------------------------------"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "P_UNZu12XYfZ"
      },
      "source": [
        "## Franka Emika Panda Robot"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "yCLziOpFXdup"
      },
      "source": [
        "### Forward Kinematics"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "HWHAKftXZK3i"
      },
      "source": [
        "\n",
        "![panda.png]()\n",
        "\n",
        "\n",
        "[Reference Link](https://www.semanticscholar.org/paper/Extracting-feasible-robot-parameters-from-dynamic-Gaz-Flacco/9daa694e11d9d62fa7fbce5c36b854a6a9413237/figure/1)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 12,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "H5d-G-9mXduq",
        "outputId": "a00ae1a6-3791-4cdb-d051-68137d0567da"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "ERobot: panda (by Franka Emika), 7 joints (RRRRRRR), 1 gripper, geometry, collision\n",
            "┌─────┬──────────────┬───────┬─────────────┬────────────────────────────────────────────────┐\n",
            "│link │     link     │ joint │   parent    │              ETS: parent to link               │\n",
            "├─────┼──────────────┼───────┼─────────────┼────────────────────────────────────────────────┤\n",
            "│   0\u001b[0m │ \u001b[38;5;4mpanda_link0\u001b[0m  │      \u001b[0m │ BASE\u001b[0m        │ SE3()\u001b[0m                                          │\n",
            "│   1\u001b[0m │ panda_link1\u001b[0m  │     0\u001b[0m │ panda_link0\u001b[0m │ SE3(0, 0, 0.333) ⊕ Rz(q0)\u001b[0m                      │\n",
            "│   2\u001b[0m │ panda_link2\u001b[0m  │     1\u001b[0m │ panda_link1\u001b[0m │ SE3(-90°, -0°, 0°) ⊕ Rz(q1)\u001b[0m                    │\n",
            "│   3\u001b[0m │ panda_link3\u001b[0m  │     2\u001b[0m │ panda_link2\u001b[0m │ SE3(0, -0.316, 0; 90°, -0°, 0°) ⊕ Rz(q2)\u001b[0m       │\n",
            "│   4\u001b[0m │ panda_link4\u001b[0m  │     3\u001b[0m │ panda_link3\u001b[0m │ SE3(0.0825, 0, 0; 90°, -0°, 0°) ⊕ Rz(q3)\u001b[0m       │\n",
            "│   5\u001b[0m │ panda_link5\u001b[0m  │     4\u001b[0m │ panda_link4\u001b[0m │ SE3(-0.0825, 0.384, 0; -90°, -0°, 0°) ⊕ Rz(q4)\u001b[0m │\n",
            "│   6\u001b[0m │ panda_link6\u001b[0m  │     5\u001b[0m │ panda_link5\u001b[0m │ SE3(90°, -0°, 0°) ⊕ Rz(q5)\u001b[0m                     │\n",
            "│   7\u001b[0m │ panda_link7\u001b[0m  │     6\u001b[0m │ panda_link6\u001b[0m │ SE3(0.088, 0, 0; 90°, -0°, 0°) ⊕ Rz(q6)\u001b[0m        │\n",
            "│   8\u001b[0m │ \u001b[38;5;4m@panda_link8\u001b[0m │      \u001b[0m │ panda_link7\u001b[0m │ SE3(0, 0, 0.107)\u001b[0m                               │\n",
            "└─────┴──────────────┴───────┴─────────────┴────────────────────────────────────────────────┘\n",
            "\n",
            "┌─────┬─────┬────────┬─────┬───────┬─────┬───────┬──────┐\n",
            "│name │ q0  │ q1     │ q2  │ q3    │ q4  │ q5    │ q6   │\n",
            "├─────┼─────┼────────┼─────┼───────┼─────┼───────┼──────┤\n",
            "│  qr\u001b[0m │  0°\u001b[0m │ -17.2°\u001b[0m │  0°\u001b[0m │ -126°\u001b[0m │  0°\u001b[0m │  115°\u001b[0m │  45°\u001b[0m │\n",
            "│  qz\u001b[0m │  0°\u001b[0m │  0°\u001b[0m    │  0°\u001b[0m │  0°\u001b[0m   │  0°\u001b[0m │  0°\u001b[0m   │  0°\u001b[0m  │\n",
            "└─────┴─────┴────────┴─────┴───────┴─────┴───────┴──────┘\n",
            "\n",
            "  \u001b[38;5;1m 0.7071  \u001b[0m \u001b[38;5;1m 0.7071  \u001b[0m \u001b[38;5;1m 0       \u001b[0m \u001b[38;5;4m 0.088   \u001b[0m  \u001b[0m\n",
            "  \u001b[38;5;1m 0.7071  \u001b[0m \u001b[38;5;1m-0.7071  \u001b[0m \u001b[38;5;1m 0       \u001b[0m \u001b[38;5;4m 0       \u001b[0m  \u001b[0m\n",
            "  \u001b[38;5;1m 0       \u001b[0m \u001b[38;5;1m 0       \u001b[0m \u001b[38;5;1m-1       \u001b[0m \u001b[38;5;4m 0.926   \u001b[0m  \u001b[0m\n",
            "  \u001b[38;5;244m 0       \u001b[0m \u001b[38;5;244m 0       \u001b[0m \u001b[38;5;244m 0       \u001b[0m \u001b[38;5;244m 1       \u001b[0m  \u001b[0m\n",
            "\n"
          ]
        }
      ],
      "source": [
        "panda = rtb.models.URDF.Panda()\n",
        "print(panda)\n",
        "T = panda.fkine(panda.qz, end='panda_hand')\n",
        "print(T)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 13,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "nrfpgySFb9Bb",
        "outputId": "011bb269-028e-41bd-b196-ea9c427be0f1"
      },
      "outputs": [
        {
          "ename": "TypeError",
          "evalue": "'IKSolution' object is not subscriptable",
          "output_type": "error",
          "traceback": [
            "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
            "\u001b[0;31mTypeError\u001b[0m                                 Traceback (most recent call last)",
            "Cell \u001b[0;32mIn[13], line 1\u001b[0m\n\u001b[0;32m----> 1\u001b[0m T\u001b[38;5;241m=\u001b[39mpanda\u001b[38;5;241m.\u001b[39mfkine(\u001b[43mpoint_sol\u001b[49m\u001b[43m[\u001b[49m\u001b[38;5;241;43m0\u001b[39;49m\u001b[43m]\u001b[49m)\n\u001b[1;32m      2\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mTransformation matrix containing same translation part\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[1;32m      3\u001b[0m \u001b[38;5;28mprint\u001b[39m(T)\n",
            "\u001b[0;31mTypeError\u001b[0m: 'IKSolution' object is not subscriptable"
          ]
        }
      ],
      "source": [
        "T=panda.fkine(point_sol[0])\n",
        "print(\"Transformation matrix containing same translation part\\n\")\n",
        "print(T)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 15,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "eH83XsYAXdus",
        "outputId": "5b1a45ca-8dfe-4a48-cf3f-6e27509b790d"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "[0.52359878 0.         0.         0.         0.         0.\n",
            " 0.        ]\n",
            "Transformation Matrix :\n",
            "   \u001b[38;5;1m 0.2588  \u001b[0m \u001b[38;5;1m 0.9659  \u001b[0m \u001b[38;5;1m 0       \u001b[0m \u001b[38;5;4m 0.07621 \u001b[0m  \u001b[0m\n",
            "  \u001b[38;5;1m 0.9659  \u001b[0m \u001b[38;5;1m-0.2588  \u001b[0m \u001b[38;5;1m 0       \u001b[0m \u001b[38;5;4m 0.044   \u001b[0m  \u001b[0m\n",
            "  \u001b[38;5;1m 0       \u001b[0m \u001b[38;5;1m 0       \u001b[0m \u001b[38;5;1m-1       \u001b[0m \u001b[38;5;4m 0.8226  \u001b[0m  \u001b[0m\n",
            "  \u001b[38;5;244m 0       \u001b[0m \u001b[38;5;244m 0       \u001b[0m \u001b[38;5;244m 0       \u001b[0m \u001b[38;5;244m 1       \u001b[0m  \u001b[0m\n",
            "\n"
          ]
        }
      ],
      "source": [
        "angles=np.array([30,00,0,0,0,0,0])\n",
        "angles_rad=np.radians(angles)\n",
        "print(angles_rad)\n",
        "T=panda.fkine(angles_rad)\n",
        "print(\"Transformation Matrix :\\n\",T)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "haMGTlmL7W0k"
      },
      "source": [
        "### Inverse Kinematics Solver"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 14,
      "metadata": {
        "id": "XDtwV5Zg7WOn"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Inverse Kinematics Solution :\n",
            " IKSolution: q=[-1.015, 2.329, 1.866, -0.438, 0.97, 1.043, 1.304], success=False, reason=iteration and search limit reached, iterations=3000, searches=100, residual=0.0126\n"
          ]
        }
      ],
      "source": [
        "point = SE3(0.6, -0.5, 0.0)\n",
        "point_sol = panda.ikine_LM(point)\n",
        "print(\"\\nInverse Kinematics Solution :\\n\" ,point_sol)\n"
      ]
    },
    {
      "attachments": {},
      "cell_type": "markdown",
      "metadata": {
        "id": "VdhHFXEtGakV"
      },
      "source": [
        "# Custom Robotic Arm Kinematics"
      ]
    },
    {
      "attachments": {},
      "cell_type": "markdown",
      "metadata": {
        "id": "yaPVvJGwXVFU"
      },
      "source": [
        "## Bazu Robotic Arm"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "ij1sdIREUVRi"
      },
      "source": [
        "### Forward Kinematics"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 16,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "K4BVCSCkDv0W",
        "outputId": "d574099b-774b-4a4a-c0c6-1227531a6927"
      },
      "outputs": [
        {
          "data": {
            "text/plain": [
              "DHRobot: , 3 joints (RRR), dynamics, standard DH parameters\n",
              "┌────┬─────┬─────┬───────┐\n",
              "│θⱼ  │ dⱼ  │ aⱼ  │  ⍺ⱼ   │\n",
              "├────┼─────┼─────┼───────┤\n",
              "│ q1\u001b[0m │ 0.5\u001b[0m │   0\u001b[0m │ 90.0°\u001b[0m │\n",
              "│ q2\u001b[0m │   0\u001b[0m │ 0.4\u001b[0m │  0.0°\u001b[0m │\n",
              "│ q3\u001b[0m │   0\u001b[0m │ 0.4\u001b[0m │  0.0°\u001b[0m │\n",
              "└────┴─────┴─────┴───────┘\n",
              "\n",
              "┌─┬──┐\n",
              "└─┴──┘"
            ]
          },
          "execution_count": 16,
          "metadata": {},
          "output_type": "execute_result"
        }
      ],
      "source": [
        "## Creating Robotic arm through defining links and Serial Linkage\n",
        "Link_1=rtb.DHLink(0.5, math.pi/2, 0, 0)\n",
        "Link_2=rtb.DHLink(0,    0,   0, 0.4)\n",
        "Link_3=rtb.DHLink(0,    0,   0, 0.4)\n",
        "bazu_robot= rtb.DHRobot([Link_1 ,Link_2,Link_3])\n",
        "bazu_robot"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 17,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "Ezd9ruOmHDRD",
        "outputId": "a1d34bc3-3bae-4335-d662-60969ce978bd"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Transformation Matrix :\n",
            "   \u001b[38;5;1m 0.5567  \u001b[0m \u001b[38;5;1m-0.3214  \u001b[0m \u001b[38;5;1m 0.766   \u001b[0m \u001b[38;5;4m 0.4453  \u001b[0m  \u001b[0m\n",
            "  \u001b[38;5;1m 0.6634  \u001b[0m \u001b[38;5;1m-0.383   \u001b[0m \u001b[38;5;1m-0.6428  \u001b[0m \u001b[38;5;4m 0.5307  \u001b[0m  \u001b[0m\n",
            "  \u001b[38;5;1m 0.5     \u001b[0m \u001b[38;5;1m 0.866   \u001b[0m \u001b[38;5;1m 0       \u001b[0m \u001b[38;5;4m 0.9     \u001b[0m  \u001b[0m\n",
            "  \u001b[38;5;244m 0       \u001b[0m \u001b[38;5;244m 0       \u001b[0m \u001b[38;5;244m 0       \u001b[0m \u001b[38;5;244m 1       \u001b[0m  \u001b[0m\n",
            "\n"
          ]
        }
      ],
      "source": [
        "##Forward Kinematics\n",
        "\n",
        "q1=50\n",
        "q2=30\n",
        "q3=0\n",
        "\n",
        "T=bazu_robot.fkine([math.radians(q1),math.radians(q2),math.radians(q3)])\n",
        "\n",
        "print(\"Transformation Matrix :\\n\",T)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "fJKhyvgnUaMZ"
      },
      "source": [
        "### Inverse Kinematics"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 18,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "ks1Yl5kUUfmL",
        "outputId": "b416ceb3-48ff-46bd-c1ea-8fa53bf2ba32"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "point-> x: 1.50 ,y: 2.50 ,z: 2.30\n",
            "IKSolution: q=[-2.874, 1.326, 1.669], success=False, reason=iteration and search limit reached, iterations=3000, searches=100, residual=1.36\n"
          ]
        }
      ],
      "source": [
        "# Selection a point to get inverse kinematics solution as angles\n",
        "print(\"point-> x: %2.2f ,y: %2.2f ,z: %2.2f\" %(1.5,2.5,2.3) )\n",
        "point = SE3( 0.4453 , 0.5307 , 0.9  )\n",
        "point_sol = bazu_robot.ikine_LM(point)\n",
        "print(point_sol)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "EX0J5FRWZwlX"
      },
      "source": [
        "### Making things symbolic for implementation"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 19,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "GSngjNT4dlMY",
        "outputId": "cab5443f-ee34-435c-9dce-de76968ad8bb"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "symblic Links :  (l_1, l_2, l_3)\n"
          ]
        }
      ],
      "source": [
        "L = sym.symbol('l_1:4') # Symbolics for links\n",
        "print(\"symblic Links : \",L)\n",
        "Link_1=rtb.DHLink(L[0], math.pi/2, 0, 0)\n",
        "Link_2=rtb.DHLink(0,    0,   0, L[1])\n",
        "Link_3=rtb.DHLink(0,    0,   0, L[2])\n",
        "Kaka_robot_symbolic= rtb.DHRobot([Link_1 ,Link_2,Link_3])"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 20,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "y8kZGulvGe7T",
        "outputId": "4642a6ac-abb5-4d85-e500-aa37ae1d2842"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "symblic Angles :  (q1, q2, q3)\n"
          ]
        }
      ],
      "source": [
        "Q= sym.symbol('q1:4')   # Symbolics for rotations angles\n",
        "print(\"symblic Angles : \",Q)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 21,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "2ZWcaT9PdFuS",
        "outputId": "c8f6f985-392d-429c-e588-d3c42a5b1a30"
      },
      "outputs": [
        {
          "data": {
            "text/plain": [
              "  \u001b[38;5;1m(-6.12323399573677e-17*sin(q1)*sin(q2) + cos(q1)*cos(q2))*cos(q3) + (-6.12323399573677e-17*sin(q1)*cos(q2) - 1.0*sin(q2)*cos(q1))*sin(q3)\u001b[0m \u001b[38;5;1m-1.0*(-6.12323399573677e-17*sin(q1)*sin(q2) + cos(q1)*cos(q2))*sin(q3) + 1.0*(-6.12323399573677e-17*sin(q1)*cos(q2) - 1.0*sin(q2)*cos(q1))*cos(q3)\u001b[0m \u001b[38;5;1m1.0*sin(q1) \u001b[0m \u001b[38;5;4m-6.12323399573677e-17*l_2*sin(q1)*sin(q2) + l_2*cos(q1)*cos(q2) + l_3*(-6.12323399573677e-17*sin(q1)*sin(q2) + cos(q1)*cos(q2))*cos(q3) + l_3*(-6.12323399573677e-17*sin(q1)*cos(q2) - 1.0*sin(q2)*cos(q1))*sin(q3)\u001b[0m  \u001b[0m\n",
              "  \u001b[38;5;1m(-1.0*sin(q1)*sin(q2) + 6.12323399573677e-17*cos(q1)*cos(q2))*sin(q3) + (sin(q1)*cos(q2) + 6.12323399573677e-17*sin(q2)*cos(q1))*cos(q3)\u001b[0m \u001b[38;5;1m1.0*(-1.0*sin(q1)*sin(q2) + 6.12323399573677e-17*cos(q1)*cos(q2))*cos(q3) - 1.0*(sin(q1)*cos(q2) + 6.12323399573677e-17*sin(q2)*cos(q1))*sin(q3)\u001b[0m \u001b[38;5;1m-1.0*cos(q1)\u001b[0m \u001b[38;5;4ml_2*sin(q1)*cos(q2) + 6.12323399573677e-17*l_2*sin(q2)*cos(q1) + l_3*(-1.0*sin(q1)*sin(q2) + 6.12323399573677e-17*cos(q1)*cos(q2))*sin(q3) + l_3*(sin(q1)*cos(q2) + 6.12323399573677e-17*sin(q2)*cos(q1))*cos(q3)\u001b[0m  \u001b[0m\n",
              "  \u001b[38;5;1m1.0*sin(q2)*cos(q3) + 1.0*sin(q3)*cos(q2)\u001b[0m \u001b[38;5;1m-1.0*sin(q2)*sin(q3) + 1.0*cos(q2)*cos(q3)\u001b[0m \u001b[38;5;1m6.12323399573677e-17\u001b[0m \u001b[38;5;4ml_1 + 1.0*l_2*sin(q2) + 1.0*l_3*sin(q2)*cos(q3) + 1.0*l_3*sin(q3)*cos(q2)\u001b[0m  \u001b[0m\n",
              "  \u001b[38;5;244m0           \u001b[0m \u001b[38;5;244m0           \u001b[0m \u001b[38;5;244m0           \u001b[0m \u001b[38;5;244m1           \u001b[0m  \u001b[0m\n"
            ]
          },
          "execution_count": 21,
          "metadata": {},
          "output_type": "execute_result"
        }
      ],
      "source": [
        "point = SE3(0.6, -0.5, 0.0)\n",
        "# point_sol = puma.ikine_LM(point)\n",
        "T_symbolic=Kaka_robot_symbolic.fkine(Q)\n",
        "T_symbolic"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 22,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 118
        },
        "id": "ShrDTAaVQHxp",
        "outputId": "be17193e-ca53-4057-bbe5-cc9a7b9fc64f"
      },
      "outputs": [
        {
          "ename": "NameError",
          "evalue": "name 'Matrix' is not defined",
          "output_type": "error",
          "traceback": [
            "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
            "\u001b[0;31mNameError\u001b[0m                                 Traceback (most recent call last)",
            "Cell \u001b[0;32mIn[22], line 2\u001b[0m\n\u001b[1;32m      1\u001b[0m Ts_symbolic \u001b[38;5;241m=\u001b[39m T_symbolic\u001b[38;5;241m.\u001b[39msimplify()\n\u001b[0;32m----> 2\u001b[0m M \u001b[38;5;241m=\u001b[39m \u001b[43mMatrix\u001b[49m(Ts_symbolic\u001b[38;5;241m.\u001b[39mA)\n\u001b[1;32m      3\u001b[0m M\n",
            "\u001b[0;31mNameError\u001b[0m: name 'Matrix' is not defined"
          ]
        }
      ],
      "source": [
        "Ts_symbolic = T_symbolic.simplify()\n",
        "M = Matrix(Ts_symbolic.A)\n",
        "M"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 24,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 78
        },
        "id": "4FyXFk-Fenqj",
        "outputId": "d81e3f3e-a8c2-4fbb-852c-7e4f3c1527a3"
      },
      "outputs": [
        {
          "ename": "NameError",
          "evalue": "name 'M' is not defined",
          "output_type": "error",
          "traceback": [
            "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
            "\u001b[0;31mNameError\u001b[0m                                 Traceback (most recent call last)",
            "Cell \u001b[0;32mIn[24], line 1\u001b[0m\n\u001b[0;32m----> 1\u001b[0m \u001b[43mM\u001b[49m[:\u001b[38;5;241m3\u001b[39m,\u001b[38;5;241m3\u001b[39m] \u001b[38;5;66;03m# extracting translation part\u001b[39;00m\n",
            "\u001b[0;31mNameError\u001b[0m: name 'M' is not defined"
          ]
        }
      ],
      "source": [
        "M[:3,3] # extracting translation part"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 23,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "XQT6fnrWQUro",
        "outputId": "7a3b0fc9-a2bc-4423-f17b-ba75ff8886ba"
      },
      "outputs": [
        {
          "ename": "NameError",
          "evalue": "name 'Kaka_robot' is not defined",
          "output_type": "error",
          "traceback": [
            "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
            "\u001b[0;31mNameError\u001b[0m                                 Traceback (most recent call last)",
            "Cell \u001b[0;32mIn[23], line 1\u001b[0m\n\u001b[0;32m----> 1\u001b[0m T\u001b[38;5;241m=\u001b[39m\u001b[43mKaka_robot\u001b[49m\u001b[38;5;241m.\u001b[39mfkine([\u001b[38;5;241m40\u001b[39m,\u001b[38;5;241m30\u001b[39m,\u001b[38;5;241m10\u001b[39m])  \u001b[38;5;66;03m# NON symbolic\u001b[39;00m\n\u001b[1;32m      2\u001b[0m T\n",
            "\u001b[0;31mNameError\u001b[0m: name 'Kaka_robot' is not defined"
          ]
        }
      ],
      "source": [
        "T=Kaka_robot.fkine([40,30,10])  # NON symbolic\n",
        "T"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "5zK9wwFXfVJe"
      },
      "source": [
        "**OUR FINAL RESULTS TO IMPLEMENT IN CODE**\n",
        "\n",
        "- X = l_2*cos(q1)*cos(q2) + l_3*cos(q1)*cos(q2 + q3)\n",
        "- Y = l_2*sin(q1)*cos(q2) + l_3*sin(q1)*cos(q2 + q3)\n",
        "- Z = l_1 + l_2*sin(q2)   + l_3*sin(q2 + q3)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "i1Up1Kzcl0IC"
      },
      "source": [
        "## Testing URDF"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "ryWQiRspmPqA"
      },
      "outputs": [],
      "source": [
        "import roboticstoolbox as rtb\n",
        "robot = rtb.models.DH.Panda()\n",
        "print(robot)\n",
        "T = robot.fkine(robot.qz)  # forward kinematics\n",
        "print(T)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 21,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "Is9WaPb3qv08",
        "outputId": "e0c75ea0-945e-4387-b632-f3c8ac3cfeb2"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "IKsolution(q=array([ 0.21343385,  1.8669104 , -0.22639717,  0.48252627,  0.2197902 ,  1.3958973 , -2.03679145]), success=True, reason=None, iterations=12, residual=1.4517611973679024e-11)\n",
            "  \u001b[38;5;1m-1       \u001b[0m \u001b[38;5;1m 9.436e-14\u001b[0m \u001b[38;5;1m 2.439e-12\u001b[0m \u001b[38;5;4m 0.7     \u001b[0m  \u001b[0m\n",
            "  \u001b[38;5;1m 9.445e-14\u001b[0m \u001b[38;5;1m 1       \u001b[0m \u001b[38;5;1m 7.257e-13\u001b[0m \u001b[38;5;4m 0.2     \u001b[0m  \u001b[0m\n",
            "  \u001b[38;5;1m-2.439e-12\u001b[0m \u001b[38;5;1m 7.257e-13\u001b[0m \u001b[38;5;1m-1       \u001b[0m \u001b[38;5;4m 0.1     \u001b[0m  \u001b[0m\n",
            "  \u001b[38;5;244m 0       \u001b[0m \u001b[38;5;244m 0       \u001b[0m \u001b[38;5;244m 0       \u001b[0m \u001b[38;5;244m 1       \u001b[0m  \u001b[0m\n",
            "\n"
          ]
        }
      ],
      "source": [
        "from spatialmath import SE3\n",
        "\n",
        "T = SE3(0.7, 0.2, 0.1) * SE3.OA([0, 1, 0], [0, 0, -1])\n",
        "sol = robot.ikine_LM(T)         # solve IK\n",
        "print(sol)\n",
        "q_pickup = sol.q\n",
        "print(robot.fkine(q_pickup))"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 26,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 741
        },
        "id": "xS_OYANvq1VI",
        "outputId": "26903e7a-c09f-49bd-8e14-f7d1f0cb808e"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "ERobot: panda (by Franka Emika), 7 joints (RRRRRRR), geometry, collision\n",
            "┌───┬──────────────┬───────┬─────────────┬─────────────────────────────────────────────────────────────────────────────┐\n",
            "│id │     link     │ joint │   parent    │                                     ETS                                     │\n",
            "├───┼──────────────┼───────┼─────────────┼─────────────────────────────────────────────────────────────────────────────┤\n",
            "│ 1\u001b[0m │ \u001b[38;5;4mpanda_link0\u001b[0m  │      \u001b[0m │ BASE\u001b[0m        │ {panda_link0} = {BASE}\u001b[0m                                                      │\n",
            "│ 2\u001b[0m │ panda_link1\u001b[0m  │     0\u001b[0m │ panda_link0\u001b[0m │ {panda_link1} = {panda_link0} ⊕ tz(0.333) ⊕ Rz(q0)\u001b[0m                          │\n",
            "│ 3\u001b[0m │ panda_link2\u001b[0m  │     1\u001b[0m │ panda_link1\u001b[0m │ {panda_link2} = {panda_link1} ⊕ Rx(-90°) ⊕ Rz(q1)\u001b[0m                           │\n",
            "│ 4\u001b[0m │ panda_link3\u001b[0m  │     2\u001b[0m │ panda_link2\u001b[0m │ {panda_link3} = {panda_link2} ⊕ ty(-0.316) ⊕ Rx(90°) ⊕ Rz(q2)\u001b[0m               │\n",
            "│ 5\u001b[0m │ panda_link4\u001b[0m  │     3\u001b[0m │ panda_link3\u001b[0m │ {panda_link4} = {panda_link3} ⊕ tx(0.0825) ⊕ Rx(90°) ⊕ Rz(q3)\u001b[0m               │\n",
            "│ 6\u001b[0m │ panda_link5\u001b[0m  │     4\u001b[0m │ panda_link4\u001b[0m │ {panda_link5} = {panda_link4} ⊕ tx(-0.0825) ⊕ ty(0.384) ⊕ Rx(-90°) ⊕ Rz(q4)\u001b[0m │\n",
            "│ 7\u001b[0m │ panda_link6\u001b[0m  │     5\u001b[0m │ panda_link5\u001b[0m │ {panda_link6} = {panda_link5} ⊕ Rx(90°) ⊕ Rz(q5)\u001b[0m                            │\n",
            "│ 8\u001b[0m │ panda_link7\u001b[0m  │     6\u001b[0m │ panda_link6\u001b[0m │ {panda_link7} = {panda_link6} ⊕ tx(0.088) ⊕ Rx(90°) ⊕ Rz(q6)\u001b[0m                │\n",
            "│ 9\u001b[0m │ \u001b[38;5;4m@panda_link8\u001b[0m │      \u001b[0m │ panda_link7\u001b[0m │ {panda_link8} = {panda_link7} ⊕ tz(0.107)\u001b[0m                                   │\n",
            "└───┴──────────────┴───────┴─────────────┴─────────────────────────────────────────────────────────────────────────────┘\n",
            "\n",
            "┌─────┬─────┬────────┬─────┬───────┬─────┬───────┬──────┐\n",
            "│name │ q0  │ q1     │ q2  │ q3    │ q4  │ q5    │ q6   │\n",
            "├─────┼─────┼────────┼─────┼───────┼─────┼───────┼──────┤\n",
            "│  qz\u001b[0m │  0°\u001b[0m │  0°\u001b[0m    │  0°\u001b[0m │  0°\u001b[0m   │  0°\u001b[0m │  0°\u001b[0m   │  0°\u001b[0m  │\n",
            "│  qr\u001b[0m │  0°\u001b[0m │ -17.2°\u001b[0m │  0°\u001b[0m │ -126°\u001b[0m │  0°\u001b[0m │  115°\u001b[0m │  45°\u001b[0m │\n",
            "└─────┴─────┴────────┴─────┴───────┴─────┴───────┴──────┘\n",
            "\n"
          ]
        },
        {
          "ename": "ModuleNotFoundError",
          "evalue": "ignored",
          "output_type": "error",
          "traceback": [
            "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
            "\u001b[0;31mModuleNotFoundError\u001b[0m                       Traceback (most recent call last)",
            "\u001b[0;32m<ipython-input-26-ef911006f1b2>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m()\u001b[0m\n\u001b[1;32m      1\u001b[0m \u001b[0mrobot\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mrtb\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mmodels\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mURDF\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mPanda\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m  \u001b[0;31m# load URDF version of the Panda\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m      2\u001b[0m \u001b[0mprint\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mrobot\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m----> 3\u001b[0;31m \u001b[0;32mfrom\u001b[0m \u001b[0mroboticstoolbox\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mbackends\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mSwift\u001b[0m \u001b[0;32mimport\u001b[0m \u001b[0mSwift\u001b[0m  \u001b[0;31m# instantiate 3D browser-based visualizer\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m      4\u001b[0m \u001b[0mbackend\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mSwift\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m      5\u001b[0m \u001b[0mbackend\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mlaunch\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m            \u001b[0;31m# activate it\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
            "\u001b[0;31mModuleNotFoundError\u001b[0m: No module named 'roboticstoolbox.backends.Swift'",
            "",
            "\u001b[0;31m---------------------------------------------------------------------------\u001b[0;32m\nNOTE: If your import is failing due to a missing package, you can\nmanually install dependencies using either !pip or !apt.\n\nTo view examples of installing some common dependencies, click the\n\"Open Examples\" button below.\n\u001b[0;31m---------------------------------------------------------------------------\u001b[0m\n"
          ]
        }
      ],
      "source": [
        "theta = np.array([0., 0., -0.25 * pi, 0., 0., 0.])\n",
        "f = robot.forward(theta)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "IdG5Gb16r9wZ",
        "outputId": "a17ccf1c-23d9-4d5a-95c9-38efb95a3a05"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "-------forward-------\n",
            "end frame t_4_4:\n",
            "[[ 7.07106781e-01 -7.07106781e-01 -2.09060286e-16 -4.97096067e-01]\n",
            " [-1.65762483e-16  1.29893408e-16 -1.00000000e+00 -2.95300000e-01]\n",
            " [ 7.07106781e-01  7.07106781e-01 -2.53632657e-17  1.29209607e+00]\n",
            " [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]\n",
            "end frame xyz:\n",
            "[-0.49709607 -0.2953      1.29209607]\n",
            "end frame abc:\n",
            "[-1.57009246e-16 -7.85398163e-01  1.57079633e+00]\n",
            "end frame rotational matrix:\n",
            "[[ 7.07106781e-01 -7.07106781e-01 -2.09060286e-16]\n",
            " [-1.65762483e-16  1.29893408e-16 -1.00000000e+00]\n",
            " [ 7.07106781e-01  7.07106781e-01 -2.53632657e-17]]\n",
            "end frame quaternion:\n",
            "[ 0.65328148 -0.27059805  0.27059805  0.65328148]\n",
            "end frame angle-axis:\n",
            "[ 1.48218982 -0.61394313  0.61394313]\n"
          ]
        }
      ],
      "source": [
        "print(\"-------forward-------\")\n",
        "print(\"end frame t_4_4:\")\n",
        "print(f.t_4_4)\n",
        "print(\"end frame xyz:\")\n",
        "print(f.t_3_1.reshape([3, ]))\n",
        "print(\"end frame abc:\")\n",
        "print(f.euler_3)\n",
        "print(\"end frame rotational matrix:\")\n",
        "print(f.r_3_3)\n",
        "print(\"end frame quaternion:\")\n",
        "print(f.q_4)\n",
        "print(\"end frame angle-axis:\")\n",
        "print(f.r_3)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "Asc6Df3NsDC4",
        "outputId": "4a328289-6501-45f3-b6a6-ee44e652ede1"
      },
      "outputs": [
        {
          "data": {
            "text/plain": [
              "array([ 0.79760367, -0.72234533, -0.92168697,  1.37145959, -1.57072942,\n",
              "       -2.34399122])"
            ]
          },
          "execution_count": 44,
          "metadata": {},
          "output_type": "execute_result"
        }
      ],
      "source": [
        "xyz = np.array([[0.28127], [0.], [1.13182]])\n",
        "abc = np.array([0.5 * pi, 0., pi])\n",
        "end = Frame.from_euler_3(abc, xyz)\n",
        "robot.inverse(end)\n",
        "print(\"inverse is successful: {0}\".format(robot.is_reachable_inverse))\n",
        "print(\"axis values: \\n{0}\".format(robot.axis_values))\n",
        "robot.show()"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "r_iIfm5lsHol",
        "outputId": "73da2648-8411-4a6b-e838-b20fe35c49cd"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "inverse is successful: True\n",
            "axis values: \n",
            "[ 0.79760367 -0.72234533 -0.92168697  1.37145959 -1.57072942 -2.34399122]\n"
          ]
        },
        {
          "name": "stderr",
          "output_type": "stream",
          "text": [
            "/usr/local/lib/python3.7/dist-packages/numpy/core/_asarray.py:136: VisibleDeprecationWarning: Creating an ndarray from ragged nested sequences (which is a list-or-tuple of lists-or-tuples-or ndarrays with different lengths or shapes) is deprecated. If you meant to do this, you must specify 'dtype=object' when creating the ndarray\n",
            "  return array(a, dtype, copy=False, order=order, subok=True)\n"
          ]
        }
      ],
      "source": [
        "print(\"inverse is successful: {0}\".format(robot.is_reachable_inverse))\n",
        "print(\"axis values: \\n{0}\".format(robot.axis_values))\n",
        "robot.show()"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "URgyfPo9sJII"
      },
      "outputs": [],
      "source": []
    }
  ],
  "metadata": {
    "colab": {
      "collapsed_sections": [
        "rhJRulJsTguA",
        "yaPVvJGwXVFU",
        "EX0J5FRWZwlX"
      ],
      "name": "Robotics_kinematics_solver.ipynb",
      "provenance": []
    },
    "kernelspec": {
      "display_name": "Python 3",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.10.12"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}
